#include "AS5600.h"
#include "esp_log.h"
#include <math.h>
#include "math_common.h"

#define BIT_RESOLUTION        12
#define AS5600_I2C_ADDR       0x36
#define AS5600_ANGLE_REGISTER 0x0C
#define DATA_START_BIT        11

const _iq24 _sensor_angle_res = _IQ(_2PI / ((1 << 12) - 1));

typedef struct
{
    char             name[20];
    uint16_t         cpr;    //!< Maximum range of the magnetic sensor
    uint16_t         lsbUsed;//!< Number of bits used in LSB register
    uint8_t          lsbMask;
    uint8_t          msbMask;
    I2CDev          *i2c;
    uint8_t          isRead;
    i2c_cmd_handle_t angleCmd;
    uint8_t          angleBuf[2];
} AS5600Obj;

void AS5600_delete(AS5600Dev *dev)
{
    AS5600Obj *priv = NULL;

    if (dev)
    {
        if (dev->priv)
        {
            priv = dev->priv;
            I2C_delDev(priv->i2c->busNum);
            free(priv);
        }
        free(dev);
    }
}

static void AS5600_getConf(AS5600Dev *dev)
{
    AS5600Obj *priv       = dev->priv;
    char       readBuf[2] = {0};
    char       reg        = 0;
    I2CPacket  wx, rx;
    uint16_t   rawData = 0;

    wx.addr            = AS5600_I2C_ADDR;
    wx.buf             = &reg;
    wx.i2cNum          = priv->i2c->busNum;
    wx.size            = 1;
    reg                = 0x07;
    priv->i2c->write(wx);

    /*读数据*/
    rx.addr   = AS5600_I2C_ADDR;
    rx.buf    = readBuf;
    rx.i2cNum = priv->i2c->busNum;
    rx.size   = 2;
    priv->i2c->read(rx);
    printf("conf: %2x,%2x\n", readBuf[0], readBuf[1]);
}

static _iq24 AS5600_getAngle(AS5600Dev *dev)
{
    AS5600Obj *priv = dev->priv;
    char       reg  = 0;
    I2CPacket  wx;
    uint16_t   rawData = 0;

    if (!priv->isRead)
    {
        /*第一次才需要准备读angle寄存器*/
        wx.addr   = AS5600_I2C_ADDR;
        wx.buf    = &reg;
        wx.i2cNum = priv->i2c->busNum;
        wx.size   = 1;
        reg       = AS5600_ANGLE_REGISTER;
        priv->i2c->write(wx);
        priv->isRead = 1;
    }

    /*读数据*/
    // rx.addr   = AS5600_I2C_ADDR;
    // rx.buf    = priv->angleBuf;
    // rx.i2cNum = priv->i2c->busNum;
    // rx.size   = 2;
    // priv->i2c->read(rx);
    priv->i2c->readFromCmd(priv->i2c->busNum, priv->angleCmd);

    rawData = (priv->angleBuf[1] & priv->lsbMask);
    rawData += ((priv->angleBuf[0] & priv->msbMask) << priv->lsbUsed);

    return _IQmpyI32(_sensor_angle_res, rawData);
}

int AS5600_cmdInit(AS5600Dev *dev)
{
    AS5600Obj *priv = dev->priv;
    priv->angleCmd  = i2c_cmd_link_create();
    i2c_master_start(priv->angleCmd);
    i2c_master_write_byte(priv->angleCmd, (AS5600_I2C_ADDR << 1) | I2C_MASTER_READ, 1);
    i2c_master_read(priv->angleCmd, priv->angleBuf, 1, I2C_MASTER_ACK);
    i2c_master_read(priv->angleCmd, priv->angleBuf + 1, 1, I2C_MASTER_NACK);
    i2c_master_stop(priv->angleCmd);
    return 0;
}

int AS5600_create(int busNum, int sda, int scl, AS5600Dev **dev)
{
    AS5600Obj *priv        = NULL;
    AS5600Dev *as5600      = NULL;
    int        bitsUsedMsb = 0;

    /*申请空间*/
    as5600 = calloc(1, sizeof(AS5600Dev));
    if (as5600 == NULL)
    {
        return ESP_FAIL;
    }
    as5600->priv = calloc(1, sizeof(AS5600Obj));
    if (as5600->priv == NULL)
    {
        goto free;
    }
    priv = as5600->priv;

    /*赋值*/
    sprintf(priv->name, "AS5600[%d]", busNum);
    priv->cpr     = (1 << 12) - 1;
    bitsUsedMsb   = DATA_START_BIT - 7;
    priv->lsbUsed = BIT_RESOLUTION - bitsUsedMsb;
    priv->lsbMask = (2 << priv->lsbUsed) - 1;
    priv->msbMask = (2 << bitsUsedMsb) - 1;

    if (I2C_getDev(busNum, sda, scl, 400000, &priv->i2c))
    {
        if (I2C_createDev(busNum, sda, scl, 400000, &priv->i2c))
        {
            ESP_LOGE(priv->name, "i2c create fail\n");
            goto free;
        }
    }

    priv->isRead     = 0;

    as5600->priv     = priv;
    as5600->getAngle = AS5600_getAngle;
    AS5600_cmdInit(as5600);
    AS5600_getConf(as5600);
    *dev = as5600;
    return ESP_OK;

free:
    AS5600_delete(as5600);
    return ESP_FAIL;
}
